#include <imp_move_filter.h>
#include <imp/imp_log.h>
#ifdef DMALLOC
#include <imp/dmalloc.h>
#endif
#define DEBUG_FLAG 0
#if DEBUG_FLAG
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#endif

#define TAG "IVSMOVEFILTER"

int Count = 0;
void MorphRowFilter(const uint8_t* src, uint8_t* dst, int width, int cn,int ksize, int isUseSimd)
{
	int i;
	const uint8_t* S = (const uint8_t*)src;
	uint8_t* D = (uint8_t*)dst;

	int i0 = 0;
	const uint8_t* s;
	uint8_t m;

	//for( k = 0; k < cn; k++, S++, D++ )
	{
		if (isUseSimd) {
        assert(0);
#if 0
			v16u8 v0,v1,v2;
			for( i = i0; i <= width - 16; i += 16 )
			{
				LU1Q(v0,S+i,0);
				LU1Q(v1,S+i+1,0);
				LU1Q(v2,S+i+2,0);

				MINU(b,v0,v0,v1);
				MINU(b,v0,v0,v2);
				SU1Q(v0,D+i,0);
			}
#endif
		}
		for( i = i0; i <= width - 2; i += 2 )
		{
			s = S + i;
			m = s[cn];
			//for( j = 2; j < _ksize; j += cn )
			m = MinuOp(m, s[2]);
			D[i] = MinuOp(m, s[0]);
			D[i+cn] = MinuOp(m, s[3]);
		}

		for( ; i < width; i += cn )
		{
			s = S + i;
			m = s[0];
			//for( j = cn; j < _ksize; j += cn )
			m = MinuOp(m, s[1]);
			m = MinuOp(m, s[2]);
			D[i] = m;
		}
	}
}
#if 0
static void CalcSum(int CntRoi, v16u8* vsum_c,int* Sum)
{
	int m;
	v16u8 v0;
	for(m = 0; m < CntRoi; m++){
		if( _mx128_bnez1q(vsum_c[m]) ){
			v0 = _mx128_li_b(1);
			vsum_c[m] = _mx128_dotpu_h(v0,vsum_c[m]);
			v0 = _mx128_li_h(1);
			vsum_c[m] = _mx128_dotpu_w(v0,vsum_c[m]);
			v0 = _mx128_li_w(1);
			vsum_c[m] = _mx128_dotpu_d(v0,vsum_c[m]);
			Sum[m] += _mx128_mtcpuu_w(vsum_c[m], 0);
			Sum[m] += _mx128_mtcpuu_w(vsum_c[m], 2);
		}
	}
}
#endif
int MorphColumnFilter(const uint8_t** _src, uint8_t* dst, int dststep, int count, int width,
		int ksize, MaskParamter* maskParamter, volatile int isUseSimd)
{

	int i, k, _ksize = ksize;
	const uint8_t** src = (const uint8_t**)_src;
	uint8_t* D = (uint8_t*)dst;
	int i0 = 0;
	dststep /= sizeof(D[0]);
#if 0
	v16u8 v0,v1,v2,v3;
#endif

	int CntRoi = maskParamter->CntRoi;
	int *County = &(maskParamter->County);
	int Rstep = maskParamter->Rstep;
	int Cstep = maskParamter->Cstep;
	int* Sum = maskParamter->Sum;
	int8_t* Rowmask = maskParamter->Rowmask;
	int8_t* Columnmask = maskParamter->Columnmask;

#if 0
	v16u8 vmask, vmask_x, vmask_d, index;
	v8i16 vRowmask[CntRoi*2];
	v16u8 vsum_c[CntRoi];
#endif
	int nstep;
	char mask_y, mask_ystep;
	char nsrl;
	volatile int m = 0;

	for( ; _ksize > 1 && count > 1; count -= 2, *County += 2, D += dststep*2, src += 2 ){
		i = i0;
		if (isUseSimd) {
        assert(0);
#if 0
			for(m = 0; m < CntRoi; m++){
				vsum_c[m] = _mx128_li_b(0);
				mask_y = (Rowmask[*County+Rstep*(m>>3)] >> (m%8))&1;
				mask_ystep = (Rowmask[*County+1+Rstep*(m>>3)] >> (m%8))&1;
				vRowmask[m] = _mx128_mfcpu_b(mask_y);
				vRowmask[m + CntRoi] = _mx128_mfcpu_b(mask_ystep);
			}

			for( ; i <= width - 16; i += 16 ){
				LU1Q(v0,src[0] + i,0);
				LU1Q(v1,src[1] + i,0);
				LU1Q(v2,src[2] + i,0);
				LU1Q(v3,src[3] + i,0);

				MINU(b,v1,v1,v2);
				MINU(b,v0,v0,v1);
				MINU(b,v3,v3,v1);

				SU1Q(v0,D+i,0);
				SU1Q(v3,D+i+dststep,0);


				for(m = 0; m < CntRoi; m ++){
					index = _mx128_mfcpu_b(m%8);
					vmask_d = _mx128_lu1q(Columnmask + (m>>3)*Cstep + i, 0);
					vmask_x = _mx128_srl_b(vmask_d, index);

					vmask = _mx128_andv(vmask_x, vRowmask[m]);
					vmask = _mx128_andv(vmask, v0);
					vsum_c[m] = _mx128_adduu_b(vsum_c[m], vmask);//width=1080 max_v5[i] = 2*1080/16 = 135;

					vmask = _mx128_andv(vmask_x, vRowmask[m + CntRoi]);
					vmask = _mx128_andv(vmask, v3);
					vsum_c[m] = _mx128_adduu_b(vsum_c[m], vmask);

				}

			}

			CalcSum(CntRoi,vsum_c,Sum);
#endif
		}//if (isUseSimd)
		for( ; i <= width - 4; i += 4 ){
			const uint8_t* sptr = src[1] + i;
			uint8_t s0 = sptr[0], s1 = sptr[1], s2 = sptr[2], s3 = sptr[3];

			//for( k = 2; k < _ksize; k++ )
			{
				sptr = src[2] + i;
				s0 = MinuOp(s0, sptr[0]); s1 = MinuOp(s1, sptr[1]);
				s2 = MinuOp(s2, sptr[2]); s3 = MinuOp(s3, sptr[3]);
			}

			sptr = src[0] + i;
			D[i] = MinuOp(s0, sptr[0]);
			D[i+1] = MinuOp(s1, sptr[1]);
			D[i+2] = MinuOp(s2, sptr[2]);
			D[i+3] = MinuOp(s3, sptr[3]);

			sptr = src[3] + i;
			D[i+dststep] = MinuOp(s0, sptr[0]);
			D[i+dststep+1] = MinuOp(s1, sptr[1]);
			D[i+dststep+2] = MinuOp(s2, sptr[2]);
			D[i+dststep+3] = MinuOp(s3, sptr[3]);

			for(m = 0; m < CntRoi; m++){
				nsrl = m%8;
				mask_y = (Rowmask[*County+Rstep*(m>>3)]>>nsrl);
				mask_ystep = (Rowmask[*County+1+Rstep*(m>>3)]>>nsrl);
				nstep = (m>>3)*Cstep;
				Sum[m] += D[i] & (Columnmask[i+nstep] >> nsrl) & mask_y;
				Sum[m] += D[i+1] & (Columnmask[i+1+nstep] >> nsrl) & mask_y;
				Sum[m] += D[i+2] & (Columnmask[i+2+nstep] >> nsrl) & mask_y;
				Sum[m] += D[i+3] & (Columnmask[i+3+nstep] >> nsrl) & mask_y;
				Sum[m] += D[i+dststep] & (Columnmask[i+nstep] >> nsrl) & mask_ystep;
				Sum[m] += D[i+dststep+1] & (Columnmask[i+1+nstep] >> nsrl) & mask_ystep;
				Sum[m] += D[i+dststep+2] & (Columnmask[i+2+nstep] >> nsrl) & mask_ystep;
				Sum[m] += D[i+dststep+3] & (Columnmask[i+3+nstep] >> nsrl) & mask_ystep;
			}
		}
		for( ; i < width; i++ ){
			uint8_t s0 = src[1][i];

			//for( k = 2; k < _ksize; k++ )
			s0 = MinuOp(s0, src[2][i]);

			D[i] = MinuOp(s0, src[0][i]);
			D[i+dststep] = MinuOp(s0, src[3][i]);

			for(m = 0;m < CntRoi; m ++){
				nsrl = m%8;
				mask_y = (Rowmask[*County+Rstep*(m>>3)]>>nsrl);
				mask_ystep = (Rowmask[*County+1+Rstep*(m>>3)]>>nsrl);
				nstep = (m>>3)*Cstep;
				Sum[m] += D[i] & (Columnmask[i+nstep] >> nsrl) & mask_y;
				Sum[m] += D[i+dststep] & (Columnmask[i+nstep] >> nsrl) & mask_ystep;
			}
		}

	}

	for(; count > 0; count--, *County++, D += dststep, src++) {
		i = i0;
		if (isUseSimd) {
        assert(0);
#if 0
			for(m = 0; m < CntRoi; m++){
				vsum_c[m] = _mx128_li_b(0);
				mask_y = (Rowmask[*County+Rstep*(m>>3)] >> (m%8))&1;
				vRowmask[m] = _mx128_mfcpu_b(mask_y);
			}
			for( ; i <= width - 16; i += 16 ){
				LU1Q(v0,src[0] + i,0);
				LU1Q(v1,src[1] + i,0);
				LU1Q(v2,src[2] + i,0);

				MINU(b,v0,v0,v0);
				MINU(b,v0,v0,v2);

				SU1Q(v0,D+i,0);

				for(m = 0; m < CntRoi; m ++){
					index = _mx128_mfcpu_b(m%8);
					vmask_d = _mx128_lu1q(Columnmask + (m>>3)*Cstep + i, 0);
					vmask_x = _mx128_srl_b(vmask_d, index);

					vmask = _mx128_andv(vmask_x, vRowmask[m]);
					vmask = _mx128_andv(vmask, v0);
					vsum_c[m] = _mx128_adduu_b(vsum_c[m], vmask);
				}
			}
			CalcSum(CntRoi,vsum_c,Sum);
#endif
		}//if (isUseSimd)
		for( ; i <= width - 4; i += 4 ){
			const uint8_t* sptr = src[0] + i;
			uint8_t s0 = sptr[0], s1 = sptr[1], s2 = sptr[2], s3 = sptr[3];

			for( k = 1; k < _ksize; k++ )
			{
				sptr = src[k] + i;
				s0 = MinuOp(s0, sptr[0]); s1 = MinuOp(s1, sptr[1]);
				s2 = MinuOp(s2, sptr[2]); s3 = MinuOp(s3, sptr[3]);
			}

			D[i] = s0; D[i+1] = s1;
			D[i+2] = s2; D[i+3] = s3;

			for(m = 0; m < CntRoi; m ++){
				nsrl = m%8;
				mask_y = (Rowmask[*County+Rstep*(m>>3)]>>nsrl);
				nstep = (m>>3)*Cstep;
				Sum[m] += D[i] & (Columnmask[i+nstep] >> nsrl) & mask_y;
				Sum[m] += D[i+1] & (Columnmask[i+1+nstep] >> nsrl) & mask_y;
				Sum[m] += D[i+2] & (Columnmask[i+2+nstep] >> nsrl) & mask_y;
				Sum[m] += D[i+3] & (Columnmask[i+3+nstep] >> nsrl) & mask_y;
			}
		}
		for( ; i < width; i++ ){
			uint8_t s0 = src[0][i];
			for( k = 1; k < _ksize; k++ )
				s0 = MinuOp(s0, src[k][i]);
			D[i] = s0;

			for(m = 0;m < CntRoi; m ++){
				nsrl = m%8;
				mask_y = (Rowmask[*County+Rstep*(m>>3)]>>nsrl);
				nstep = (m>>3)*Cstep;
				Sum[m] += D[i] & (Columnmask[i+nstep] >> nsrl) & mask_y;
			}
		}
	}

	return 0;
}

#if DEBUG_FLAG
int file_src = 0;
int kcount = 0;
#endif
int move_detect(Image src1,Image src2,Image dst,int* rets,FilterEngine* filterEngine, int isUseSimd)
{
	if(filterEngine == NULL) {
		IMP_LOG_ERR(TAG, "filterEngine is NULL\n");
		return -1;
	}

	filterEngine->rowCount = 0;
	filterEngine->dstY = 0;

	zRect zRoi = filterEngine->roi;
	filterEngine->startY = MaxsOp(zRoi.y - 1, 0);
	filterEngine->startY0 = MaxsOp(zRoi.y - 1, 0);
	filterEngine->endY = MinsOp(zRoi.y + zRoi.height + 3 - 1 - 1, src1.height);
	memset((void*)filterEngine->maskParamter.Sum,0,filterEngine->maskParamter.CntRoi*sizeof(int));
	filterEngine->maskParamter.County = 0;
	filterEngine->rowCount = 0;
	filterEngine->dstY = 0;
	//printf("%d,%d,%d,[%d,%d,%d,%d]\n",filterEngine->startY, filterEngine->startY0,filterEngine->endY,zRoi.x,zRoi.y,zRoi.width,zRoi.height);

	uint8_t* ptr_src1_roi = src1.data + zRoi.x + zRoi.y*src1.step;
	uint8_t* ptr_src2_roi = src2.data + zRoi.x + zRoi.y*src2.step;
	uint8_t* ptr_dst_roi = dst.data + zRoi.x + zRoi.y*dst.step;

	Proceed( ptr_src1_roi, src1.step, ptr_src2_roi, src2.step, ptr_dst_roi,
			dst.step, filterEngine, isUseSimd);
#if DEBUG_FLAG
	int k = 0, t = 0;
	int sum_k = 0;
	uint8_t dsrc[zRoi.height][zRoi.width];
	for(k = 0;k<zRoi.height;k++){
		uint8_t* data1 = dst.data + (zRoi.y + k)*dst.step + zRoi.x;
		uint8_t* data2 = src2.data + (zRoi.y + k)*src2.step + zRoi.x;
		for(t = 0;t<zRoi.width;t++){
			//dsrc[k][t] = (abs(data1[t] - data2[t]) > 20) ? 255:0;
			dsrc[k][t] = (data1[t] > 0) ? 255:0 ;
			//dsrc[k][t] = data1[t];
			sum_k += (dsrc[k][t]>0);
		}
	}
	//printf("sum = %d\n",sum_k);
	file_src = open("src_new.data",O_RDWR|O_CREAT|O_APPEND);
	write(file_src,&dsrc[0][0],zRoi.height*zRoi.width);
	kcount++;
	if(kcount==100){
		close(file_src);
		printf("close file\n");
		return 0;
	}
#endif
	int cnt = 0;
	if(isUseSimd){
        assert(0);
#if 0
		v16u8 vsum, vthresh;
		for(cnt = 0;cnt <= filterEngine->maskParamter.CntRoi-4; cnt+=4){
			LU1Q(vsum, filterEngine->maskParamter.Sum+cnt, 0);
			LU1Q(vthresh, filterEngine->Noise_Thresh+cnt,0);
			CLTU(w,vsum,vthresh,vsum);
			SRLI(w,vsum,vsum,31);
			SU1Q(vsum,rets+cnt,0);
			//printf("rets = [%d, %d, %d, %d], %d\n",filterEngine->maskParamter.Sum[0],filterEngine->maskParamter.Sum[1],filterEngine->maskParamter.Sum[2],filterEngine->maskParamter.Sum[3]);
		}
#endif
	}
	for(;cnt < filterEngine->maskParamter.CntRoi; cnt++){
		rets[cnt] = filterEngine->maskParamter.Sum[cnt] > filterEngine->Noise_Thresh[cnt];
		//printf("ret[%d] = %d, ",cnt,filterEngine->maskParamter.Sum[cnt]);
	}
	//printf("\n");

	return 0;
}
